# 用于处理从传感器获取的原始数据

from format_str import now_to_str_ms
from config import ProjectInfo

gyroRange = 2000.0  # 角速度量程
accRange = 16.0  # 加速度量程
angleRange = 180.0  # 角度量程
TempFindValues = []  # 读取指定寄存器返回的数据


# region 传感器原始数据处理


# 将传感器数据转为数字
def sensor_raw_to_int(data_str):
    int_value_list = []
    if data_str and len(data_str) == 108:
        if ProjectInfo.Sensor_data_Record_state:
            ProjectInfo.sensor_raw_data_queue.put(data_str)
        for i in range(54):
            data = data_str[i * 2:i * 2 + 2]
            data_value = int(data, 16)
            int_value_list.append(data_value)
    return int_value_list


# region 解析传感器数据
def get_acc(data_hex):
    """
    加速度、温度结算
    :param data_hex: 原始始数据包
    :param deviceModel: 设备模型
    :return:
    """
    axl = data_hex[0]
    axh = data_hex[1]
    ayl = data_hex[2]
    ayh = data_hex[3]
    azl = data_hex[4]
    azh = data_hex[5]

    acc_x = (axh << 8 | axl) / 32768.0 * accRange
    acc_y = (ayh << 8 | ayl) / 32768.0 * accRange
    acc_z = (azh << 8 | azl) / 32768.0 * accRange
    if acc_x >= accRange:
        acc_x -= 2 * accRange
    if acc_y >= accRange:
        acc_y -= 2 * accRange
    if acc_z >= accRange:
        acc_z -= 2 * accRange

    return [acc_x, acc_y, acc_z]


def get_gyro(data_hex):
    """
    角速度结算
    :param data_hex: 原始始数据包
    :return:
    """
    wxl = data_hex[0]
    wxh = data_hex[1]
    wyl = data_hex[2]
    wyh = data_hex[3]
    wzl = data_hex[4]
    wzh = data_hex[5]

    gyro_x = (wxh << 8 | wxl) / 32768.0 * gyroRange
    gyro_y = (wyh << 8 | wyl) / 32768.0 * gyroRange
    gyro_z = (wzh << 8 | wzl) / 32768.0 * gyroRange
    if gyro_x >= gyroRange:
        gyro_x -= 2 * gyroRange
    if gyro_y >= gyroRange:
        gyro_y -= 2 * gyroRange
    if gyro_z >= gyroRange:
        gyro_z -= 2 * gyroRange
    return [gyro_x, gyro_y, gyro_z]


def get_angle(data_hex):
    """
    角度结算
    :param data_hex: 原始始数据包
    :param deviceModel: 设备模型
    :return:
    """
    rxl = data_hex[0]
    rxh = data_hex[1]
    ryl = data_hex[2]
    ryh = data_hex[3]
    rzl = data_hex[3]
    rzh = data_hex[5]

    angle_x = (rxh << 8 | rxl) / 32768.0 * angleRange
    angle_y = (ryh << 8 | ryl) / 32768.0 * angleRange
    angle_z = (rzh << 8 | rzl) / 32768.0 * angleRange
    if angle_x >= angleRange:
        angle_x -= 2 * angleRange
    if angle_y >= angleRange:
        angle_y -= 2 * angleRange
    if angle_z >= angleRange:
        angle_z -= 2 * angleRange

    return [round(angle_x, 3), round(angle_y, 3), round(angle_z, 3)]


def get_mag(data_hex):
    """
    磁场结算
    :param data_hex: 原始始数据包
    :return:返回磁场的值
    """
    _x = get_int(bytes([data_hex[0], data_hex[1]]))
    _y = get_int(bytes([data_hex[2], data_hex[3]]))
    _z = get_int(bytes([data_hex[4], data_hex[5]]))
    return [_x, _y, _z]


def get_int(data_bytes):
    """
    int转换有符号整形   = C# BitConverter.ToInt16
    :param data_bytes: 字节数组
    :return:
    """
    # return -(data & 0x8000) | (data & 0x7fff)
    return int.from_bytes(data_bytes, "little", signed=True)


def get_four_elements(data_hex):
    """
    四元素结算
    :param data_hex: 原始始数据包
    :return:
    """
    q1 = get_int(bytes([data_hex[2], data_hex[3]])) / 32768.0
    q2 = get_int(bytes([data_hex[4], data_hex[5]])) / 32768.0
    q3 = get_int(bytes([data_hex[6], data_hex[7]])) / 32768.0
    q4 = get_int(bytes([data_hex[8], data_hex[9]])) / 32768.0
    return [q1, q2, q3, q4]


def get_sensor_id_by_hex(data_hex):
    wt_id = ""
    for i in range(12):
        wt_id += chr(data_hex[i])
    return wt_id


def get_date(data_hex):
    # 日期和时间
    return "20" + str(data_hex[12]) + "-" + str(data_hex[13]) + "-" + str(data_hex[14])


def get_time(data_hex):
    time_ms = (data_hex[19] << 8) | data_hex[18]
    return str(data_hex[15]) + ":" + str(data_hex[16]) + ":" + str(data_hex[17]) + ":" + str(time_ms)


# endregion


def tread_sensor_values(int_value_list):
    value_dic = {}

    if int_value_list:
        value_dic["sensor_code"] = get_sensor_id_by_hex(int_value_list)

        # 日期和时间
        # value_dic["d_date"] = today_to_str()
        value_dic["d_time"] = now_to_str_ms()
        # 加速度
        acc = get_acc(int_value_list[20:26])
        value_dic["acc_x"] = acc[0]
        value_dic["acc_y"] = acc[1]
        value_dic["acc_z"] = acc[2]

        # 角速度
        gyro = get_gyro(int_value_list[26:32])
        value_dic["gyro_x"] = gyro[0]
        value_dic["gyro_y"] = gyro[1]
        value_dic["gyro_z"] = gyro[2]

        # 磁场
        mag = get_mag(int_value_list[32:38])
        value_dic["mag_x"] = mag[0]
        value_dic["mag_y"] = mag[1]
        value_dic["mag_z"] = mag[2]

        # 角度
        angle = get_angle(int_value_list[38:44])
        value_dic["angle_x"] = angle[0]
        value_dic["angle_y"] = angle[1]
        value_dic["angle_z"] = angle[2]
        # 温度
        temp = ((int_value_list[45] << 8) | int_value_list[44]) / 100
        value_dic["temp"] = temp
        bat = ((int_value_list[47] << 8) | int_value_list[46]) / 100
        value_dic["bat"] = bat
        rss = (int_value_list[49] << 8) | int_value_list[48]
        value_dic["rss"] = rss
    return value_dic


def get_sensor_data_list(int_value_list):
    value_dic = tread_sensor_values(int_value_list)
    col_name_list = ["sensor_code", "d_date", "d_time", "acc_x", "acc_y", "acc_z", "gyro_x", "gyro_y", "gyro_z",
                     "mag_x", "mag_y", "mag_z", "angle_x", "angle_y", "angle_z", "temp", "bat", "rss"]
    return dic_to_list(col_name_list, value_dic)


def dic_to_list(col_name_list: list, data_dic: dict):
    if data_dic:
        data_list = []
        for col_name in col_name_list:
            data_list.append(data_dic[col_name])
        return data_list
    else:
        return []


def get_sensor_data(raw_data, data_type="list"):
    int_data = sensor_raw_to_int(raw_data)
    if data_type == "dict":
        return tread_sensor_values(int_data)
    if data_type == "list":
        return get_sensor_data_list(int_data)


# endregion

